#include "CalcCamParameter.hpp" 
using namespace std;


CalcCamParameter::CalcCamParameter() {}
CalcCamParameter::~CalcCamParameter() {}

std::deque<CalcCamParameter::CamParameter> camParams;

void CalcCamParameter::calculate(std::string inputFileName) {
	std::ifstream inputFile(inputFileName.c_str());

	if(!inputFile.is_open()){
	}

	std::string currentLine;
	std::string currentToken;
	std::string tmpStr;
	std::stringstream sstr;
	int tmpPos1, tmpPos2;
	int length;
	CamParameter tmpCamParams;
	char buffer[100];

	std::getline(inputFile, currentLine);

	while(!inputFile.eof()) {
		std::getline(inputFile, currentLine);
		tmpPos1=(int)currentLine.find(",");
		length = currentLine.copy(buffer,tmpPos1,0);
		buffer[length]='\0';
		cout<<"filename:"<<buffer<<endl;
		tmpCamParams.filename=buffer;

		tmpPos1=(int)currentLine.find(",",tmpPos1+1);
		tmpPos2=(int)currentLine.find(",",tmpPos1+1);
		length = currentLine.copy(buffer,tmpPos2-tmpPos1-1,tmpPos1+1);
		buffer[length]='\0';
		cout<<"lat:"<<buffer<<endl;
		tmpCamParams.lattitude = atof(buffer);

		tmpPos1=(int)currentLine.find(",",tmpPos2+1);
		length = currentLine.copy(buffer,tmpPos1-tmpPos2-1,tmpPos2+1);
		buffer[length]='\0';
		cout<<"lon:"<<buffer<<endl;
		tmpCamParams.longitude = atof(buffer);

		tmpPos1=(int)currentLine.find(",",tmpPos1+1);
		tmpPos2=(int)currentLine.find(",",tmpPos1+1);
		length = currentLine.copy(buffer,tmpPos2-tmpPos1-1,tmpPos1+1);
		buffer[length]='\0';
		cout<<"alt:"<<buffer<<endl;
		tmpCamParams.alt = atof(buffer);

		tmpPos1=(int)currentLine.find(",",tmpPos2+1);
		length = currentLine.copy(buffer,tmpPos1-tmpPos2-1,tmpPos2+1);
		buffer[length]='\0';
		cout<<"heading:"<<buffer<<endl;
		tmpCamParams.heading = atof(buffer);

		tmpPos2=(int)currentLine.find(",",tmpPos1+1);
		length = currentLine.copy(buffer,tmpPos2-tmpPos1-1,tmpPos1+1);
		buffer[length]='\0';
		cout<<"drift:"<<buffer<<endl;
		tmpCamParams.drift = atof(buffer);

		tmpPos1=tmpPos2;
		tmpPos2=(int)currentLine.find(",",tmpPos2+1);
		length = currentLine.copy(buffer,tmpPos2-tmpPos1-1,tmpPos1+1);
		buffer[length]='\0';
		cout<<"roll:"<<buffer<<endl;
		tmpCamParams.roll = atof(buffer);

		tmpPos1=tmpPos2;
		tmpPos2=(int)currentLine.find(",",tmpPos2+1);
		length = currentLine.copy(buffer,tmpPos2-tmpPos1-1,tmpPos1+1);
		buffer[length]='\0';
		cout<<"pitch:"<<buffer<<"\n"<<endl;
		tmpCamParams.pitch = atof(buffer);

		camParams.push_back(tmpCamParams);
	}
}

void CalcCamParameter::writeToFile() {
	ofstream output;
	std::stringstream data;

	output.open("camParam.csv",ios::app);

	data<<"filename,T[0][0],T[1][0],T[2][0],T[0][1],T[1][1],T[2][1],T[0][2],T[1][2],T[2][2],X,Y,Z";

	for (int i=0; i<camParams.size(); ++i) {
		data<<"\n"<<camParams[i].filename<<",";
		calcT(i,data);
		calcXYZ(i,data);
	}
	
	output<<data.str()<<endl;
	output.close();

}

void CalcCamParameter::calcT(int pos, std::stringstream &data) {
	float omega, phi, kappa;
	float T[3][3];
	
	omega = camParams[pos].roll / 180 * PI;
	phi = -1 * camParams[pos].pitch / 180 * PI;
	kappa = (90 - camParams[pos].heading - camParams[pos].drift) / 180 * PI;


	T[0][0] = cos(phi)*cos(kappa);
	data<<T[0][0]<<",";
	T[1][0] = -1 * cos(phi)*sin(kappa);
	data<<T[1][0]<<",";
	T[2][0] = sin(phi);
	data<<T[2][0]<<",";
	T[0][1] = cos(omega)*sin(kappa) + sin(omega)*sin(phi)*cos(kappa);
	data<<T[0][1]<<",";
	T[1][1] = cos(omega)*cos(kappa) - sin(omega)*sin(phi)*sin(kappa);
	data<<T[1][1]<<",";
	T[2][2] = cos(omega)*cos(phi);
	data<<T[2][2]<<",";
	T[0][2] = sin(omega)*sin(kappa) - cos(omega)*sin(phi)*cos(kappa);
	data<<T[0][2]<<",";
	T[1][2] = sin(omega)*cos(kappa) + cos(omega)*sin(phi)*sin(kappa);
	data<<T[1][2]<<",";
	T[2][1] = -1 * sin(omega)*cos(phi);
	data<<T[2][1]<<",";

}

void CalcCamParameter::calcXYZ(int pos, std::stringstream &data) {
	double x,y,z;
	convertLatLonToUTM(camParams[pos].lattitude,camParams[pos].longitude,x,y);
	z=camParams[pos].alt*0.3048;
	data<<x<<","<<y<<","<<z<<endl;

}

void CalcCamParameter::getFocalLength(int picType) {
	int focalLength=0;
	switch(picType)
	{ 
	case 0:  //telelens
		focalLength = 610;
		break;
	case 1:  //trilens
		focalLength = 80;
		break;
	case 2: //pentalens
		focalLength = 57;
	}

}

void CalcCamParameter::convertLatLonToUTM(const double Lat, const double Long, 
						double &UTMNorthing, double &UTMEasting) {
	//converts lat/long to UTM coords.  Equations from USGS Bulletin 1532 
	//East Longitudes are positive, West longitudes are negative. 
	//North latitudes are positive, South latitudes are negative
	//Lat and Long are in decimal degrees
	//Written by Chuck Gantz- chuck.gantz@globalstar.com

	double a = 6377563;
	double eccSquared = 0.00667054;
	double k0 = 0.9996;
	double deg2rad = 360/2*PI;

	double LongOrigin;
	double eccPrimeSquared;
	double N, T, C, A, M;

	//Make sure the longitude is between -180.00 .. 179.9
	double LongTemp = (Long+180)-int((Long+180)/360)*360-180; // -180.00 .. 179.9;

	double LatRad = Lat* deg2rad;
	double LongRad = LongTemp*deg2rad;
	double LongOriginRad;
	int    ZoneNumber;

	ZoneNumber = int((LongTemp + 180)/6) + 1;

	if( Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 )
		ZoneNumber = 32;

	// Special zones for Svalbard
	if( Lat >= 72.0 && Lat < 84.0 ) 
	{
		if(      LongTemp >= 0.0  && LongTemp <  9.0 ) ZoneNumber = 31;
		else if( LongTemp >= 9.0  && LongTemp < 21.0 ) ZoneNumber = 33;
		else if( LongTemp >= 21.0 && LongTemp < 33.0 ) ZoneNumber = 35;
		else if( LongTemp >= 33.0 && LongTemp < 42.0 ) ZoneNumber = 37;
	}
	LongOrigin = (ZoneNumber - 1)*6 - 180 + 3;  //+3 puts origin in middle of zone
	LongOriginRad = LongOrigin * deg2rad;

	//compute the UTM Zone from the latitude and longitude
	//sprintf(UTMZone, "%d%c", ZoneNumber, UTMLetterDesignator(Lat));

	eccPrimeSquared = (eccSquared)/(1-eccSquared);

	N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad));
	T = tan(LatRad)*tan(LatRad);
	C = eccPrimeSquared*cos(LatRad)*cos(LatRad);
	A = cos(LatRad)*(LongRad-LongOriginRad);

	M = a*((1	- eccSquared/4		- 3*eccSquared*eccSquared/64	- 5*eccSquared*eccSquared*eccSquared/256)*LatRad 
		- (3*eccSquared/8	+ 3*eccSquared*eccSquared/32	+ 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad)
		+ (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) 
		- (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad));

	UTMEasting = (double)(k0*N*(A+(1-T+C)*A*A*A/6
		+ (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120)
		+ 500000.0);
	
	cout<<UTMEasting<<" - ";

	UTMNorthing = (double)(k0*(M+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
		+ (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720)));
	if(Lat < 0)
		UTMNorthing += 10000000.0; //10000000 meter offset for southern hemisphere
	cout<<UTMNorthing<<endl;
}